/*
 Copyright (c) 2013 MeherTJ G. All rights reserved.
 License: LGPL for personnal study or free software.
 */

/// Quaternion

#pragma once

#include <cmath>
#include <functional>
#include "zen_number.h"
#include "zen_vector.h"

namespace Zen {
	typedef Vec4 Quat;
}

namespace Zen {

	inline Quat QuatIdentity()
	{
		return { 0.f, 0.f, 0.f, 1.f };
	}
	/*
	 x, y, and z represent the imaginary values.
	 */
	inline Quat QuatMake(float x, float y, float z, float w);

	/*
	 vector represents the imaginary values.
	 */
	inline Quat QuatMake(Vec3 vector, float scalar);

	/*
	 Assumes the axis is already normalized.
	 */
	inline Quat QuatMakeWithAngleAndAxis(float radians, Vec3 axis);

	/*
	 Calculate and return the angle component of the angle and axis form.
	 */
	float QuatAngle(Quat const & quat);

	/*
	 Calculate and return the axis component of the angle and axis form.
	 */
	Vec3 QuatGetAxis(Quat const & quat);

	inline Quat QuatMultiply(Quat left, Quat right);

	inline float QuatLength(Quat const & quat);
	inline float QuatLength2(Quat const & quat);

	inline Quat QuatConjugate(Quat const & quat);
	inline Quat QuatInvert(Quat const & quat);
	inline Quat QuatNormalize(Quat const & quat);

	inline Vec3 QuatRotateVec3(Quat const & quat, Vec3 vector);
	void QuatRotateVec3Array(Quat const & quat, Vec3 * vectors, size_t vectorCount);

	/*
	 The fourth component of the vector is ignored when calculating the rotation.
	 */
	inline Vec4 QuatRotateVec4(Quat const & quat, Vec4 vector);
	void QuatRotateVec4Array(Quat const & quat, Vec4 * vectors, size_t vectorCount);

#pragma mark -
#pragma mark Implementations
#pragma mark -

	inline Quat QuatMake(float x, float y, float z, float w)
	{
		Quat q = { x, y, z, w };
		return q;
	}

	inline Quat QuatMake(Vec3 vector, float scalar)
	{
		Quat q = { vector[0], vector[1], vector[2], scalar };
		return q;
	}

	inline Quat QuatMakeWithAngleAndAxis(float radians, cVec3 & axis)
	{
		float halfAngle = radians * 0.5f;
		float scale = sinf(halfAngle);
		Quat q = { scale * axis.x, scale * axis.y, scale * axis.z, cosf(halfAngle) };
		return q;
	}
	inline Quat QuatMultiply(Quat left, Quat right)
	{

		return { left[3] * right[0] +
			left[0] * right[3] +
			left[1] * right[2] -
			left[2] * right[1],

			left[3] * right[1] +
			left[1] * right[3] +
			left[2] * right[0] -
			left[0] * right[2],

			left[3] * right[2] +
			left[2] * right[3] +
			left[0] * right[1] -
			left[1] * right[0],

			left[3] * right[3] -
			left[0] * right[0] -
			left[1] * right[1] -
			left[2] * right[2] };
	}

	inline float QuatLength(Quat const & quat)
	{
		return sqrt(quat[0] * quat[0] +
					quat[1] * quat[1] +
					quat[2] * quat[2] +
					quat[3] * quat[3]);
	}
	inline float QuatLength2(Quat const & quat)
	{
		return (quat[0] * quat[0] +
					quat[1] * quat[1] +
					quat[2] * quat[2] +
					quat[3] * quat[3]);
	}

	inline Quat QuatConjugate(Quat const & quat)
	{
		Quat q = { -quat[0], -quat[1], -quat[2], quat[3] };
		return q;
	}

	inline Quat QuatInvert(Quat const & quat)
	{
		float scale = (quat[0] * quat[0] +
					   quat[1] * quat[1] +
					   quat[2] * quat[2] +
					   quat[3] * quat[3]);
		scale = 1.f / scale;
		Quat q = { -quat[0] * scale, -quat[1] * scale, -quat[2] * scale, quat[3] * scale };
		return q;
	}

	inline Quat QuatNormalize(Quat const & quat)
	{
		float scale = 1.0f / QuatLength(quat);
		Quat q = { quat[0] * scale, quat[1] * scale, quat[2] * scale, quat[3] * scale };
		return q;
	}

	inline Vec3 QuatRotateVec3(Quat const & quat, Vec3 vector)
	{
		Quat rotQ = QuatMake(vector[0], vector[1], vector[2], 0.0f);
		rotQ = QuatMultiply(QuatMultiply(quat, rotQ), QuatInvert(quat));

		return Vec3Make(rotQ[0], rotQ[1], rotQ[2]);
	}

	inline Vec4 QuatRotateVec4(Quat const & quat, Vec4 vector)
	{
		Quat rotQ = QuatMake(vector[0], vector[1], vector[2], 0.0f);
		rotQ = QuatMultiply(QuatMultiply(quat, rotQ), QuatInvert(quat));

		return Vec4Make(rotQ[0], rotQ[1], rotQ[2], vector[3]);
	}
}
